#ifndef _FUSIONOBSTACLEDETECTORNODELET_H
#define _FUSIONOBSTACLEDETECTORNODELET_H

#include <iostream>
#include <memory>
#include <chrono>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointField.h>
#include <sensor_msgs/Image.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include "qomolo_msgs/ControlSequence.h"
#include "qomolo_msgs/SpeedLimit.h"
#include "qomolo_msgs/ObstacleArray.h"
#include "qomolo_msgs/Obstacle.h"
#include "logger/logger.h"
#include "module_base/module_base.h"
#include "module_base/util.h"
#include "obstacle_detector/obstacle_detector.h"
#include "obstacle_detector/util.h"

namespace obstacle_detector
{
using ChronoMicro=std::chrono::microseconds;

class ObstacleDetectorNodelet : public module_base::ModuleBase
{
public:
	ObstacleDetectorNodelet(){};
	~ObstacleDetectorNodelet(){};
	virtual void onInit();

private:
	void pointcloud_callback(
		const sensor_msgs::PointCloud2::ConstPtr& pt2_msg1,
		const sensor_msgs::PointCloud2::ConstPtr& pt2_msg2);
	void filterpc_callback(
		const sensor_msgs::PointCloud2::ConstPtr& pc_msg);
	void map_callback(const std_msgs::String::ConstPtr& map_msg);
	void odom_callback(const nav_msgs::Odometry::ConstPtr odom_msg);
	void vehicleparam_callback(const std_msgs::String::ConstPtr& map_msg);
	void control_sequence_callback(
		const qomolo_msgs::ControlSequence::ConstPtr control_sequence_msg);
	bool find_nearest_loc(module_base::Location &loc);
	void publish_speed_limit(float collision_index);
	void publish_obstacles_visual(const std::vector<Object> &obstacles);
	void publish_obstacles(const module_base::Location &loc);

private:
	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,sensor_msgs::PointCloud2> PointcloudPolicy;
	typedef message_filters::Synchronizer<PointcloudPolicy> PcSync;
	std::shared_ptr<PcSync> pc_sync_;
	message_filters::Subscriber<sensor_msgs::PointCloud2> sub_1_;
	message_filters::Subscriber<sensor_msgs::PointCloud2> sub_2_;

	std::unique_ptr<ObstacleDetector> obstacle_detector_; 
	std::shared_ptr<logger::Logger> debug_logger_;

	std::mutex loc_mutex_;
	std::vector<module_base::Location> odom_loc_list_;

	float current_speed_;

};//class
}//namespace

#endif